import numpy
from  transform import *
from ti5_kinematic import Ti5Kinematic
class tt(object):
    def __init__(self):
        self.n8 = numpy.linspace(1,-1,20)

    def get_more_point(self,xyzrpy_s,xyzrpy_e,i,m):

        li_tmp=list()
        if xyzrpy_s[i]>0 and xyzrpy_e[i]>0:
            per_distance  = abs(xyzrpy_s[i]-xyzrpy_e[i])/20
        elif xyzrpy_s[i]<0 and xyzrpy_e[i]<0:
            per_distance  = abs(xyzrpy_s[i]-xyzrpy_e[i])/20
        else:
            per_distance  = (abs(xyzrpy_s[i])+abs(xyzrpy_e[i]))/20

        if xyzrpy_s[i]>xyzrpy_e[i]:
            for ii in range(20):
                tt =ii
                t1 = per_distance*tt
                bias =1-abs(self.n8[ii])
                #print(t1+abs(1-abs(self.n8[ii]))/5)
                li_tmp.append(xyzrpy_s[i]-(t1+bias/m))
        else:
            for ii in range(20):
                tt =ii
                t1 = per_distance*tt
                bias =1-abs(self.n8[ii])

                #li_tmp.append(xyzrpy_s[i]+(t1+abs(1-abs(self.n8[ii]))*5))
                if i<3:
                    li_tmp.append(xyzrpy_s[i]+(t1+bias/m))
                else:
                    li_tmp.append(xyzrpy_s[i]+t1)
                    
        return li_tmp
    def get_per_elem(self,end_list):
        end_end=list()
        for ii in range(20):
            #for i in range(6):
            
            if 1<2:
                point_list=[0,0,0,0,0,0]
                point_list[0]=end_list[0][ii]
                point_list[1]=end_list[1][ii]
                point_list[2]=end_list[2][ii]
                point_list[3]=end_list[3][ii]
                point_list[4]=end_list[4][ii]
                point_list[5]=end_list[5][ii]
                end_end.append(point_list)
        return end_end
            #print(str(end_list[0][ii])+"+"+str(end_list[1][ii])+"+"+str(end_list[2][ii])+"+"+str(end_list[3][ii])+"+"+str(end_list[4][ii])+"+"+str(end_list[5][ii]))
    def get_best_plan(self,end_list):
        for ii in range(20):
            #for i in range(3):
            print(end_list[6][ii])

    def get_normal_plan(self,xyzrpy_s,xyzrpy_e):


        li_tmp=list()

        for t4 in range(6):
            tmp_list=list()
            if xyzrpy_s[t4]>0 and xyzrpy_e[t4]>0:
                per_distance  = abs(xyzrpy_s[t4]-xyzrpy_e[t4])/20
            elif xyzrpy_s[t4]<0 and xyzrpy_e[t4]<0:
                per_distance  = abs(xyzrpy_s[t4]-xyzrpy_e[t4])/20
            else:
                per_distance  = (abs(xyzrpy_s[t4])+abs(xyzrpy_e[t4]))/20
            if xyzrpy_s[t4]>xyzrpy_e[t4]:
                for t3 in range(20):
                    tt=t3+1
                    if t4<3:#j1 j2 j3
                        tmp_list.append(xyzrpy_s[t4]-per_distance*tt)
                    else:
                        tmp_list.append(xyzrpy_s[t4]-per_distance*20)
            else:
                for t3 in range(20):
                    tt=t3+1
                    if t4<3:#j1 j2 j3
                        tmp_list.append(xyzrpy_s[t4]+per_distance*tt)
                    else:
                        tmp_list.append(xyzrpy_s[t4]+per_distance*20)
            li_tmp.append(tmp_list)
        e =list()                
        for i in range(20):
            tmp = [0,0,0,0,0,0]
            tmp[0]=li_tmp[0][i]
            tmp[1]=li_tmp[1][i]
            tmp[2]=li_tmp[2][i]
            tmp[3]=li_tmp[3][i]
            tmp[4]=li_tmp[4][i]
            tmp[5]=li_tmp[5][i]
            e.append(tmp)

        return e




if __name__ =="__main__":
    t = tt()
    end_list = list()
    xyz_s =[0.7,0.4,-0.3,0.1,0.1,0.2]
    xyz_e = [0.1,-0.2,0.4,0.2,-0.1,0.2]
    r1 = t.get_normal_plan(xyz_s,xyz_e)
    arm_kinematic = Ti5Kinematic()
    start_joint =arm_kinematic.forward_kinematic_v2(xyz_s)
    #for iiii in r1:

    #    print(str(iiii))

    m =5
    print("s:"+str(xyz_s))
    print("e:"+str(xyz_e))
    print(str(t.n8))
    for i in range(6):
        res = t.get_more_point(xyz_s,xyz_e,i,m)
        end_list.append(res)
    #print(str(end_list))
    re = t.get_per_elem(end_list)
    #get circle point list
    #re.append(xyz_e)
    pose = Pose()

    #get line point list
    eeee = t.get_normal_plan(xyz_s,xyz_e)
    nn =0
    totle_list=list()



    for t2 in re:
        print(str(t2))
        pose.set_position(t2[0]*700,t2[1]*700,t2[2]*700)
        pose.set_euler_angle(t2[3],t2[4],t2[5])
        candi_joint_angle_list_final = arm_kinematic.inverse_kinematic(pose)
        #for i in  candi_joint_angle_list_final :
            #print(str(i))
    # for ii in range(20):
    #    print(str(end_list[0][ii])+"#"+str(end_list[1][ii])+"#"+str(end_list[2][ii]))
